package Behaviors.Schemas.Perception;

import Utils.*;
import javaclient3.*;



public class NxPSDetectObstaclesVirtualObj extends NxPSDetectObstacles {

    private Vector2d[] virtualObjCenter;
    private Position2DInterface[] obstacles;
    private float radius;
    private Position2DInterface myPos;
    
    public NxPSDetectObstaclesVirtualObj(String name,Position2DInterface myPos,Vector2d[] virtualObjCenter, float radius) {
        super(name);
        this.virtualObjCenter = virtualObjCenter;
        this.radius = radius ;
        this.myPos = myPos;


        
    }
    public NxPSDetectObstaclesVirtualObj(String name,Position2DInterface myPos,Position2DInterface[] obstacles, float radius) {
        super(name);
        this.obstacles = obstacles;
        this.radius = radius;
        this.myPos = myPos;
    }

    
    public Vector2d[] Value() {
        
        //if the obstacles were passed as Position2d types, then extract its Vector2d
        if(obstacles!=null)
        {
            virtualObjCenter = new Vector2d[obstacles.length];
            for(int x=0;x<obstacles.length;x++)
            {
                //obstacles[x].getData().getPos();
                virtualObjCenter[x] = Position2DHelper.getPosition(obstacles[x]);
                
  
            }
        }
        //extract  the real position of obstacles, taking the nearest point of the obstacle to the position of robot
        Vector2d[] value = new Vector2d[virtualObjCenter.length];
        for(int i= 0;i<value.length;i++)
        {
            //calculate vector from obstacle to position of robot
            value[i] = Position2DHelper.getPosition(myPos).sub(virtualObjCenter[i]);
            //define size of object
            value[i].setMag(radius);
            //define final position of object
            value[i] = value[i].add(virtualObjCenter[i]);
            /*
            System.out.print("x");
            System.out.print(i);
            System.out.print(": ");

            System.out.print(value[i].getX());
            System.out.print(" y");
            System.out.print(i);
            System.out.print(": ");

            System.out.println(value[i].getY());
            */
            
        }
        return value;
    }
}